/* source/core/sgl_math.c
 *
 * MIT License
 *
 * Copyright(c) 2023-present All contributors of SGL  
 * Document reference link: docs directory
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <sgl_math.h>


/**
 * @brief sine table
 * @note the table is used to calculate the sine of an angle
 */
static const int16_t sin0_90_table[] = {
    0,     572,   1144,  1715,  2286,  2856,  3425,  3993,  4560,  5126,  5690,  6252,  6813,  7371,  7927,  8481,
    9032,  9580,  10126, 10668, 11207, 11743, 12275, 12803, 13328, 13848, 14364, 14876, 15383, 15886, 16383, 16876,
    17364, 17846, 18323, 18794, 19260, 19720, 20173, 20621, 21062, 21497, 21925, 22347, 22762, 23170, 23571, 23964,
    24351, 24730, 25101, 25465, 25821, 26169, 26509, 26841, 27165, 27481, 27788, 28087, 28377, 28659, 28932, 29196,
    29451, 29697, 29934, 30162, 30381, 30591, 30791, 30982, 31163, 31335, 31498, 31650, 31794, 31927, 32051, 32165,
    32269, 32364, 32448, 32523, 32587, 32642, 32687, 32722, 32747, 32762, 32767
};


/**
 * @brief sqrt table, if the number less than 1024, the sqrt root will get by the table
 */
static const uint8_t sqrt_error_table[] = {
    0x00,0x00,0x6a,0xbb,0x00,0x3c,0x73,0xa5,0xd4,0x00,0x29,0x51,0x76,0x9b,0xbd,0xdf,
    0x00,0x1f,0x3e,0x5b,0x78,0x95,0xb0,0xcb,0xe6,0x00,0x19,0x32,0x4a,0x62,0x7a,0x91,
    0xa8,0xbe,0xd4,0xea,0x00,0x15,0x2a,0x3e,0x53,0x67,0x7b,0x8e,0xa2,0xb5,0xc8,0xdb,
    0xed,0x00,0x12,0x24,0x36,0x47,0x59,0x6a,0x7b,0x8c,0x9d,0xae,0xbe,0xcf,0xdf,0xef,
    0x00,0x0f,0x1f,0x2f,0x3f,0x4e,0x5d,0x6d,0x7c,0x8b,0x9a,0xa9,0xb7,0xc6,0xd4,0xe3,
    0xf1,0x00,0x0e,0x1c,0x2a,0x38,0x46,0x53,0x61,0x6f,0x7c,0x8a,0x97,0xa4,0xb2,0xbf,
    0xcc,0xd9,0xe6,0xf3,0x00,0x0c,0x19,0x26,0x32,0x3f,0x4b,0x58,0x64,0x70,0x7c,0x89,
    0x95,0xa1,0xad,0xb9,0xc5,0xd1,0xdc,0xe8,0xf4,0x00,0x0b,0x17,0x22,0x2e,0x39,0x44,
    0x50,0x5b,0x66,0x72,0x7d,0x88,0x93,0x9e,0xa9,0xb4,0xbf,0xca,0xd5,0xdf,0xea,0xf5,
    0x00,0x0a,0x15,0x1f,0x2a,0x34,0x3f,0x49,0x54,0x5e,0x68,0x73,0x7d,0x87,0x91,0x9c,
    0xa6,0xb0,0xba,0xc4,0xce,0xd8,0xe2,0xec,0xf6,0x00,0x09,0x13,0x1d,0x27,0x30,0x3a,
    0x44,0x4d,0x57,0x61,0x6a,0x74,0x7d,0x87,0x90,0x99,0xa3,0xac,0xb6,0xbf,0xc8,0xd1,
    0xdb,0xe4,0xed,0xf6,0x00,0x09,0x12,0x1b,0x24,0x2d,0x36,0x3f,0x48,0x51,0x5a,0x63,
    0x6c,0x74,0x7d,0x86,0x8f,0x98,0xa0,0xa9,0xb2,0xbb,0xc3,0xcc,0xd5,0xdd,0xe6,0xee,
    0xf7,0x00,0x08,0x11,0x19,0x21,0x2a,0x32,0x3b,0x43,0x4c,0x54,0x5c,0x65,0x6d,0x75,
    0x7d,0x86,0x8e,0x96,0x9e,0xa7,0xaf,0xb7,0xbf,0xc7,0xcf,0xd7,0xdf,0xe7,0xef,0xf7,
    0x00,0x07,0x0f,0x17,0x1f,0x27,0x2f,0x37,0x3f,0x47,0x4f,0x57,0x5e,0x66,0x6e,0x76,
    0x7e,0x85,0x8d,0x95,0x9c,0xa4,0xac,0xb4,0xbb,0xc3,0xca,0xd2,0xda,0xe1,0xe9,0xf0,
    0xf8,0x00,0x07,0x0f,0x16,0x1e,0x25,0x2c,0x34,0x3b,0x43,0x4a,0x52,0x59,0x60,0x68,
    0x6f,0x76,0x7e,0x85,0x8c,0x94,0x9b,0xa2,0xa9,0xb1,0xb8,0xbf,0xc6,0xcd,0xd5,0xdc,
    0xe3,0xea,0xf1,0xf8,0x00,0x07,0x0e,0x15,0x1c,0x23,0x2a,0x31,0x38,0x3f,0x46,0x4d,
    0x54,0x5b,0x62,0x69,0x70,0x77,0x7e,0x85,0x8c,0x92,0x99,0xa0,0xa7,0xae,0xb5,0xbc,
    0xc2,0xc9,0xd0,0xd7,0xde,0xe4,0xeb,0xf2,0xf9,0x00,0x06,0x0d,0x14,0x1a,0x21,0x28,
    0x2e,0x35,0x3c,0x42,0x49,0x50,0x56,0x5d,0x64,0x6a,0x71,0x77,0x7e,0x84,0x8b,0x92,
    0x98,0x9f,0xa5,0xac,0xb2,0xb9,0xbf,0xc6,0xcc,0xd3,0xd9,0xdf,0xe6,0xec,0xf3,0xf9,
    0x00,0x06,0x0c,0x13,0x19,0x1f,0x26,0x2c,0x32,0x39,0x3f,0x45,0x4c,0x52,0x58,0x5f,
    0x65,0x6b,0x71,0x78,0x7e,0x84,0x8a,0x91,0x97,0x9d,0xa3,0xa9,0xb0,0xb6,0xbc,0xc2,
    0xc8,0xcf,0xd5,0xdb,0xe1,0xe7,0xed,0xf3,0xf9,0x00,0x06,0x0c,0x12,0x18,0x1e,0x24,
    0x2a,0x30,0x36,0x3c,0x42,0x48,0x4e,0x54,0x5a,0x60,0x66,0x6c,0x72,0x78,0x7e,0x84,
    0x8a,0x90,0x96,0x9c,0xa2,0xa8,0xad,0xb3,0xb9,0xbf,0xc5,0xcb,0xd1,0xd7,0xdc,0xe2,
    0xe8,0xee,0xf4,0xfa,0x00,0x05,0x0b,0x11,0x17,0x1d,0x22,0x28,0x2e,0x34,0x39,0x3f,
    0x45,0x4b,0x50,0x56,0x5c,0x62,0x67,0x6d,0x73,0x78,0x7e,0x84,0x89,0x8f,0x95,0x9a,
    0xa0,0xa6,0xab,0xb1,0xb7,0xbc,0xc2,0xc8,0xcd,0xd3,0xd8,0xde,0xe4,0xe9,0xef,0xf4,
    0xfa,0x00,0x05,0x0b,0x10,0x16,0x1b,0x21,0x26,0x2c,0x31,0x37,0x3c,0x42,0x47,0x4d,
    0x52,0x58,0x5d,0x63,0x68,0x6e,0x73,0x79,0x7e,0x84,0x89,0x8e,0x94,0x99,0x9f,0xa4,
    0xaa,0xaf,0xb4,0xba,0xbf,0xc5,0xca,0xcf,0xd5,0xda,0xdf,0xe5,0xea,0xef,0xf5,0xfa,
    0x00,0x05,0x0a,0x0f,0x15,0x1a,0x1f,0x25,0x2a,0x2f,0x35,0x3a,0x3f,0x44,0x4a,0x4f,
    0x54,0x5a,0x5f,0x64,0x69,0x6e,0x74,0x79,0x7e,0x83,0x89,0x8e,0x93,0x98,0x9d,0xa3,
    0xa8,0xad,0xb2,0xb7,0xbd,0xc2,0xc7,0xcc,0xd1,0xd6,0xdc,0xe1,0xe6,0xeb,0xf0,0xf5,
    0xfa,0x00,0x05,0x0a,0x0f,0x14,0x19,0x1e,0x23,0x28,0x2d,0x32,0x38,0x3d,0x42,0x47,
    0x4c,0x51,0x56,0x5b,0x60,0x65,0x6a,0x6f,0x74,0x79,0x7e,0x83,0x88,0x8d,0x92,0x97,
    0x9c,0xa1,0xa6,0xab,0xb0,0xb5,0xba,0xbf,0xc4,0xc9,0xce,0xd3,0xd8,0xdd,0xe2,0xe7,
    0xec,0xf1,0xf6,0xfb,0x00,0x04,0x09,0x0e,0x13,0x18,0x1d,0x22,0x27,0x2c,0x31,0x35,
    0x3a,0x3f,0x44,0x49,0x4e,0x53,0x58,0x5c,0x61,0x66,0x6b,0x70,0x75,0x79,0x7e,0x83,
    0x88,0x8d,0x92,0x96,0x9b,0xa0,0xa5,0xaa,0xae,0xb3,0xb8,0xbd,0xc2,0xc6,0xcb,0xd0,
    0xd5,0xd9,0xde,0xe3,0xe8,0xed,0xf1,0xf6,0xfb,0x00,0x04,0x09,0x0e,0x12,0x17,0x1c,
    0x21,0x25,0x2a,0x2f,0x33,0x38,0x3d,0x42,0x46,0x4b,0x50,0x54,0x59,0x5e,0x62,0x67,
    0x6c,0x70,0x75,0x7a,0x7e,0x83,0x88,0x8c,0x91,0x96,0x9a,0x9f,0xa3,0xa8,0xad,0xb1,
    0xb6,0xbb,0xbf,0xc4,0xc8,0xcd,0xd2,0xd6,0xdb,0xdf,0xe4,0xe9,0xed,0xf2,0xf6,0xfb,
    0x00,0x04,0x09,0x0d,0x12,0x16,0x1b,0x1f,0x24,0x29,0x2d,0x32,0x36,0x3b,0x3f,0x44,
    0x48,0x4d,0x51,0x56,0x5a,0x5f,0x63,0x68,0x6c,0x71,0x75,0x7a,0x7e,0x83,0x87,0x8c,
    0x90,0x95,0x99,0x9e,0xa2,0xa7,0xab,0xb0,0xb4,0xb9,0xbd,0xc1,0xc6,0xca,0xcf,0xd3,
    0xd8,0xdc,0xe1,0xe5,0xe9,0xee,0xf2,0xf7,0xfb,0x00,0x04,0x08,0x0d,0x11,0x16,0x1a,
    0x1e,0x23,0x27,0x2c,0x30,0x34,0x39,0x3d,0x41,0x46,0x4a,0x4f,0x53,0x57,0x5c,0x60,
    0x64,0x69,0x6d,0x71,0x76,0x7a,0x7e,0x83,0x87,0x8b,0x90,0x94,0x98,0x9d,0xa1,0xa5,
    0xaa,0xae,0xb2,0xb7,0xbb,0xbf,0xc4,0xc8,0xcc,0xd0,0xd5,0xd9,0xdd,0xe2,0xe6,0xea,
    0xee,0xf3,0xf7,0xfb,0x00,0x04,0x08,0x0c,0x11,0x15,0x19,0x1d,0x22,0x26,0x2a,0x2e,
    0x33,0x37,0x3b,0x3f,0x43,0x48,0x4c,0x50,0x54,0x59,0x5d,0x61,0x65,0x69,0x6e,0x72,
    0x76,0x7a,0x7e,0x83,0x87,0x8b,0x8f,0x93,0x98,0x9c,0xa0,0xa4,0xa8,0xac,0xb1,0xb5,
    0xb9,0xbd,0xc1,0xc5,0xca,0xce,0xd2,0xd6,0xda,0xde,0xe3,0xe7,0xeb,0xef,0xf3,0xf7,
    0xfb,0x00,0x04,0x08,0x0c,0x10,0x14,0x18,0x1c,0x20,0x25,0x29,0x2d,0x31,0x35,0x39,
    0x3d,0x41,0x45,0x49,0x4e,0x52,0x56,0x5a,0x5e,0x62,0x66,0x6a,0x6e,0x72,0x76,0x7a,
    0x7e,0x83,0x87,0x8b,0x8f,0x93,0x97,0x9b,0x9f,0xa3,0xa7,0xab,0xaf,0xb3,0xb7,0xbb,
    0xbf,0xc3,0xc7,0xcb,0xcf,0xd3,0xd7,0xdb,0xdf,0xe3,0xe7,0xeb,0xef,0xf3,0xf7,0xfb,
    0x00
};


/**
 * the table will used to calculate the square root of a number
 */
static const uint32_t sqrt_error_init_table[32] = {
    0, 1, 1, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 5,
    5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7
};


/**
 * @brief Calculate the sine of an angle
 * @param angle: Angle in degrees such 0-359
 * @return sine of the angle from sin0_90_table
 */
int32_t sgl_sin(int16_t angle)
{
    int16_t ret = 0;

    while (angle >= 360) {
        angle -= 360;
    }
    while (angle < 0) {
        angle += 360;
    }

    if(angle < 0) {
        angle = 360 + angle;
    }

    if(angle < 90) {
        ret = sin0_90_table[angle];
    }
    else if(angle >= 90 && angle < 180){
        angle = 180 - angle;
        ret   = sin0_90_table[angle];
    }
    else if(angle >= 180 && angle < 270){
        angle = angle - 180;
        ret   = -sin0_90_table[angle];
    }
    else {
        angle = 360 - angle;
        ret   = -sin0_90_table[angle];
    }

    return ret;
}


/**
 * @brief  Calculate x number square root
 * @param  x: x number
 * @retval x number square root
 */
uint16_t sgl_sqrt(uint32_t x)
{
    uint32_t rem = 0;
    uint32_t root = 0;
    uint32_t divisor = 0;

    for(int i=0; i<16; i++) {
        root <<= 1;
        rem = ((rem << 2) + (x >> 30));
        x <<= 2;
        divisor = (root<<1) + 1;
        
        if(divisor <= rem) {
            rem -= divisor;
            root++;
        }
    }
    return (uint16_t)(root);
}


/**
 * @brief Calculate error of the square root for a number.
 * @param x The number to calculate the error for.
 * @return The error of the square root. [0 ~ 255]
 */
uint8_t sgl_sqrt_error(uint32_t x)
{
    uint32_t bsh = (1 << 14);
    uint32_t fpr = 0;
    uint32_t osh = 0;
    uint32_t bod;

    if(x < 1025) {
        return sqrt_error_table[x];
    }
    else if (x > (1 << 30)) {
        return 0;
    }

    int32_t index = 0;
    uint32_t xp = x;

    if ((xp & 0xFFFF0000) == 0) { index += 16; xp <<= 16; }
    if ((xp & 0xFF000000) == 0) { index += 8; xp <<= 8; }
    if ((xp & 0xF0000000) == 0) { index += 4; xp <<= 4; }
    if ((xp & 0xC0000000) == 0) { index += 2; xp <<= 2; }
    if ((xp & 0x80000000) == 0) { index += 1; }

    osh = sqrt_error_init_table[31 - index];
    bsh = 1 << (2 * osh + 14);

    do {
        bod = bsh + fpr;
        if(x >= bod) {
            x -= bod;
            fpr = bsh + bod;
        }
        x <<= 1;
    }while(bsh >>= 1);

    return fpr>>osh;
}


/**
 * @brief Calculate the angle based on the x and y coordinates. This function is a fast algorithm
 *         implementation, with reference address: www.RomanBlack.com (Fast XY vector to integer degree algorithm)
 *
 * @param x:  The x coordinate on a circle
 * @param y:  The y coordinate on a circle
 * @return angle 
 * @note return angle [-32767 ~ 32767]
 */
int32_t sgl_atan2_raw(int x, int y)
{
    int32_t ret;

    if(y == 0) {
        if(x > 0) {
            ret = -32767;
        }
        else {
            ret = 32767;
        }
    }
    else {
        ret = (x << 8) / y;
    }
    if(y > 0) {
        ret += 32767;
    }
    else {
        ret -= 32767;
    }

    return ret;
}


/**
 * @brief Calculate the angle based on the x and y coordinates. This function is a fast algorithm 
 *        implementation, with reference address: www.RomanBlack.com (Fast XY vector to integer degree algorithm)
 * 
 * @param x:  The x coordinate on a circle
 * @param y:  The y coordinate on a circle
 * @return angle
 * @note return angle [0 ~ 359]
*/
uint16_t sgl_atan2_angle(int x, int y)
{
    unsigned char negflag;
    unsigned char tempdegree;
    unsigned char comp;
    unsigned int degree;
    unsigned int ux;
    unsigned int uy;

    negflag = 0;
    if(x < 0) {
        negflag += 0x01;
        x = (0 - x);
    }
    ux = x;
    if(y < 0) {
        negflag += 0x02;
        y = (0 - y);
    }
    uy = y;

    if(ux > uy) {
        degree = (uy * 45) / ux;
        negflag += 0x10;
    }
    else{
        degree = (ux * 45) / uy;
    }

    comp = 0;
    tempdegree = degree;
    if(tempdegree > 22) {
        if(tempdegree <= 44) comp++;
        if(tempdegree <= 41) comp++;
        if(tempdegree <= 37) comp++;
        if(tempdegree <= 32) comp++;
    }
    else {
        if(tempdegree >= 2) comp++;
        if(tempdegree >= 6) comp++;
        if(tempdegree >= 10) comp++;
        if(tempdegree >= 15) comp++;
    }
    degree += comp;

    if(negflag & 0x10) degree = (90 - degree);

    if(negflag & 0x02) {
        if(negflag & 0x01)
            degree = (180 + degree);
        else
            degree = (180 - degree);
    }
    else {
        if(negflag & 0x01)
            degree = (360 - degree);
    }
    return degree;
}
